from .period_task import period_task
import lcm
from quad_bot import torques_pb


# count=0
# num=0

class lcm_subscriber(period_task):
    def __init__(self, name, period, channel):
        period_task.__init__(self, name, period)
        self.lc = lcm.LCM()
        self.channel = channel
        self.torques = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    def init(self):
        self.lc.subscribe(self.channel, self.callback)
        print("[LCM subscriber] init!")
        pass

    def run_once(self):
        self.lc.handle()
        # global count,num
        # if count>1000:
        #     count=1
        #     num+=1
        # print("subscriber run",num,count,"times")
        # count+=1

    def clean_up(self):
        # print("[LCM subscriber] clean up!")
        pass

    def callback(self, channel, data):
        self.msg = torques_pb.decode(data)
        self.torques = list(self.msg.torques)
        # print("Torques:",self.torques)
        # print(type(self.msg.torques))
        # print(type([0, -1, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0]))
